#include "EEPROM.h"
#include <string.h>
//CS PA4
//SPI1


float PID_data_array[4][3] = {
    {0, 0, 0}, // Yaw PID
    {0, 0, 0}, // Gray PID
    {0, 0, 0}, // Find Blobs PID
    {0, 0, 0}  // Go Target PID
};





uint32_t W25QXX_ReadID(void)
{
    uint8_t cmd = 0x9F;
    uint8_t id[3] = {0, 0, 0};
    
    W25QXX_CS_ON(0);
    HAL_SPI_Transmit(&hspi1, &cmd, 1, HAL_MAX_DELAY);
    HAL_SPI_Receive(&hspi1, id, 3, HAL_MAX_DELAY);
    W25QXX_CS_ON(1);
    
    // 打印芯片信息
    printf("Byte 0: 0x%02X\n", id[0]);  // 应为0xEF (Manufacturer ID)
    printf("Byte 1: 0x%02X\n", id[1]);  // Memory Type
    printf("Byte 2: 0x%02X\n", id[2]);  // Capacity
    
    uint32_t full_id = (id[0] << 16) | (id[1] << 8) | id[2];
//    printf("Complete ID: 0x%06X\n", (unsigned int)full_id);
    
    return full_id;
}


void W25Q128_write_enable(void)
{
    uint8_t cmd = 0x06; // 写使能
    W25QXX_CS_ON(0); // 选中芯片
    HAL_SPI_Transmit(&hspi1, &cmd, 1, HAL_MAX_DELAY); // 发送命令
    W25QXX_CS_ON(1); // 释放芯片
}
void W25Q128_wait_busy(void)
{
    uint8_t cmd = 0x05; // 获取状态寄存器的命令
    uint8_t status = 0;
    W25QXX_CS_ON(0); // 选中芯片
    do {
        HAL_SPI_Transmit(&hspi1, &cmd, 1, HAL_MAX_DELAY); // 发送命令
        HAL_SPI_Receive(&hspi1, &status, 1, HAL_MAX_DELAY); // 接收状态
    } while (status & 0x01); // 等待写完成
    W25QXX_CS_ON(1); // 释放芯片
}



// W25Q128的16M存储空间分为256个块(Block),每个块大小为64K字节64000字节，每个块的地址为16位(Sector),每个块4K字节。
// W25Q128的块大小为16位，也可以每次写入4K字节。
void W25Q128_erase_sector(uint32_t addr)
{
    W25Q128_write_enable(); // 写使能
    uint8_t cmd[4] = {0x20, (addr >> 16) & 0xFF, (addr >> 8) & 0xFF, addr & 0xFF}; // 发送地址
    W25QXX_CS_ON(0); // 选中芯片
    HAL_SPI_Transmit(&hspi1, cmd, 4, HAL_MAX_DELAY); // 发送地址
    W25QXX_CS_ON(1); // 释放芯片
    W25Q128_wait_busy(); // 等待写完成
}


float EEPROM_ReadFloat(uint32_t address)
{
    float value;
    uint8_t data[4];
    uint8_t cmd[4];

    cmd[0] = 0x03; // 读取
    cmd[1] = (address >> 16) & 0xFF;
    cmd[2] = (address >> 8) & 0xFF;
    cmd[3] = address & 0xFF;

    W25QXX_CS_ON(0);
    HAL_SPI_Transmit(&hspi1, cmd, 4, HAL_MAX_DELAY); // 发送地址+数据
    HAL_SPI_Receive(&hspi1, data, 4, HAL_MAX_DELAY); // 接收数据
    W25QXX_CS_ON(1);

    memcpy(&value, data, sizeof(value));
    return value;
}

void EEPROM_WriteFloat(uint32_t address, float value)
{
    uint8_t data[4];
    uint8_t cmd[4];
    memcpy(data, &value, sizeof(value)); // 转换为字节

    // 写之前先擦除，否则会写入失败
    uint32_t sector_addr = address & ~(4096 - 1); // 4KB对齐
    W25Q128_erase_sector(sector_addr);

    W25Q128_write_enable(); // 写使能
    cmd[0] = 0x02; // 页写命令
    cmd[1] = (address >> 16) & 0xFF;
    cmd[2] = (address >> 8) & 0xFF;
    cmd[3] = address & 0xFF;

    W25QXX_CS_ON(0);
    HAL_SPI_Transmit(&hspi1, cmd, 4, HAL_MAX_DELAY); // 发送地址+数据
    HAL_SPI_Transmit(&hspi1, data, 4, HAL_MAX_DELAY); // 发送数据
    W25QXX_CS_ON(1);

    W25Q128_wait_busy(); // 等待写完成
}

void W25QXX_erase_all(void) //前2048个块全部擦除
{
    for (uint32_t i = 0; i < 2048; i++)
    {
        printf("Erasing sector %d...\n", i);
        W25Q128_erase_sector(i * 4096); // 每个块4K字节
    }
    printf ("All sectors erased.\n");
}



void W25QXX_first_Init(void) //第一次写入EEPROM,写入2048个块，然后默认值覆盖
{
    __disable_irq(); // 如果正在使用中断，请注意打开注释
    printf("W25QXX first initialization...\n");
    //W25QXX_erase_all(); // 如果前200个块
    //写入前200个块
    for (uint32_t i = 0; i < 200; i++)
    {
        printf("Erasing sector %d...\n", i);
        W25Q128_erase_sector(i * 4096); // 每个块4K字节
    }
    //默认值覆盖
    EEPROM_WriteFloat(YAW_KP_ADDR,JY62_PID.Kp);
    EEPROM_WriteFloat(YAW_KI_ADDR, JY62_PID.Ki);
    EEPROM_WriteFloat(YAW_KD_ADDR, JY62_PID.Kd);

    EEPROM_WriteFloat(GRAY_KP_ADDR, sensor_track_PID.Kp);
    EEPROM_WriteFloat(GRAY_KI_ADDR, sensor_track_PID.Ki);
    EEPROM_WriteFloat(GRAY_KD_ADDR, sensor_track_PID.Kd);

    EEPROM_WriteFloat(BLOBS_KP_ADDR, 0.5f);
    EEPROM_WriteFloat(BLOBS_KI_ADDR, 0.1f);
    EEPROM_WriteFloat(BLOBS_KD_ADDR, 0.05f);

    EEPROM_WriteFloat(GO_TARGET_KP_ADDR, 0.5f);
    EEPROM_WriteFloat(GO_TARGET_KI_ADDR, 0.1f);
    EEPROM_WriteFloat(GO_TARGET_KD_ADDR, 0.05f);
    __enable_irq(); // 如果正在使用中断，请注意打开注释
    printf("First initialization complete. Default values written to EEPROM.\n");
}


void W25QXX_Init(void) //flash写入之后直接使用，不需要读取数据
{
    //读取所有的PID数据，不需要初始化PID_data_array，直接赋值给JY62_PID和sensor_track_PID
    PID_data_array[0][0] = EEPROM_ReadFloat(YAW_KP_ADDR);
    PID_data_array[0][1] = EEPROM_ReadFloat(YAW_KI_ADDR);
    PID_data_array[0][2] = EEPROM_ReadFloat(YAW_KD_ADDR);
    JY62_PID.Kp = PID_data_array[0][0];
    JY62_PID.Ki = PID_data_array[0][1];
    JY62_PID.Kd = PID_data_array[0][2];
	//printf("%.2f %.2f %.2f\n",PID_data_array[0][0],PID_data_array[0][1],PID_data_array[0][2]);
    PID_data_array[1][0] = EEPROM_ReadFloat(GRAY_KP_ADDR);
    PID_data_array[1][1] = EEPROM_ReadFloat(GRAY_KI_ADDR);
    PID_data_array[1][2] = EEPROM_ReadFloat(GRAY_KD_ADDR);
    sensor_track_PID.Kp = PID_data_array[1][0];
    sensor_track_PID.Ki = PID_data_array[1][1];
    sensor_track_PID.Kd = PID_data_array[1][2];


//    PID_data_array[2][0] = EEPROM_ReadFloat(BLOBS_KP_ADDR);
//    PID_data_array[2][1] = EEPROM_ReadFloat(BLOBS_KI_ADDR);
//    PID_data_array[2][2] = EEPROM_ReadFloat(BLOBS_KD_ADDR);
//    Find_blobs_PID.Kp = PID_data_array[2][0];
//    Find_blobs_PID.Ki = PID_data_array[2][1];
//    Find_blobs_PID.Kd = PID_data_array[2][2];

//    PID_data_array[3][0] = EEPROM_ReadFloat(GO_TARGET_KP_ADDR);
//    PID_data_array[3][1] = EEPROM_ReadFloat(GO_TARGET_KI_ADDR);
//    PID_data_array[3][2] = EEPROM_ReadFloat(GO_TARGET_KD_ADDR);
//    Go_Target_PID.Kp = PID_data_array[3][0];
//    Go_Target_PID.Ki = PID_data_array[3][1];
//    Go_Target_PID.Kd = PID_data_array[3][2];
}



//保存当前所有的PID数据到EEPROM
void W25QXX_SavePIDData(void)
{
    //判断
    __disable_irq(); // 如果正在使用中断，请注意打开注释
    // 写PID数据到EEPROM
    EEPROM_WriteFloat(YAW_KP_ADDR, PID_data_array[0][0]);
    EEPROM_WriteFloat(YAW_KI_ADDR, PID_data_array[0][1]);
    EEPROM_WriteFloat(YAW_KD_ADDR, PID_data_array[0][2]);
    printf("Saving Yaw PID data: Kp=%.2f, Ki=%.2f, Kd=%.2f\n", PID_data_array[0][0], PID_data_array[0][1], PID_data_array[0][2]);   
    EEPROM_WriteFloat(GRAY_KP_ADDR, PID_data_array[1][0]);
    EEPROM_WriteFloat(GRAY_KI_ADDR, PID_data_array[1][1]);
    EEPROM_WriteFloat(GRAY_KD_ADDR, PID_data_array[1][2]);

//    EEPROM_WriteFloat(BLOBS_KP_ADDR, PID_data_array[2][0]);
//    EEPROM_WriteFloat(BLOBS_KI_ADDR, PID_data_array[2][1]);
//    EEPROM_WriteFloat(BLOBS_KD_ADDR, PID_data_array[2][2]);

//    EEPROM_WriteFloat(GO_TARGET_KP_ADDR, PID_data_array[3][0]);
//    EEPROM_WriteFloat(GO_TARGET_KI_ADDR, PID_data_array[3][1]);
//    EEPROM_WriteFloat(GO_TARGET_KD_ADDR, PID_data_array[3][2]);
    //判断
    __enable_irq(); // 如果正在使用中断，请注意打开注释
}

